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(g) Attitude stabilization control system for legged mobile robot 



(57) A system for controlling attitude stabilization 
of a biped mobile robot. The system comprises 
means for generating a desired walking pattern 
of the robot so that a ground reaction force 
moment (Mact) acting on the robot, when the 
robot's leg comes into contact with the ground, 
is at a desired position (ZMP), and is provided 
with a first model made up of an inverted 
pendulum and simulating dynamics of the robot 
and a second model simulating the mechanical 
structure of the robot such that the second 
model is assumed to walk on the ground in 
accordance with the desired walking pattern. 
There are further provided first means for de- 
tecting an inclinatory error (Oerr) of the robot's 
trunk relative to the second model, second 
means for determining a manipulated variable 
(Mmdl) in terms of moment in response to the 
detected inclinatory error (Oerr) and supplying 
it to the first model such that the inverted 
pendulum is rotated about its pivot point by an 
amount (0mdl) corresponding to the manipu- 
lated variable (Mmdl), and third means for de- 
tecting positional variation (Ax) of an upper 
portion of the inverted pendulum (correspond- 
ing to the trunk) resulting from the rotation and 
correcting the motion of the second model in 
response to the detected positional variation 
(Ax). The robot's joints are controlled to follow 
the motion of the second model. Thus, the robot 
can move stably over the ground. 
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This invention relates to an attitude stabilization 
control system for a legged mobile robot. 

Legged mobile robots, in particular legged mobile 
robots using biped locomotion, are taught by Japa- 
nese Laid-open Patent Publication Nos. 62(1987)- 
97005 and 63(1988)-150176. A general discussion of 
robots, including legged mobile robots, can be found 
in Robotics Handbook, Robotic Society of Japan, 
October 20, 1990. 

The stability of legged mobile robots, in particular 
legged mobile robots using biped locomotion, is in- 
trinsically low. Thus when such a robot is acted on by 
external forces (disturbance), its attitude easily be- 
comes unstable. Because of this, it is necessary to 
control the robot's attitude on the basis of the detect- 
ed inclinatory angle and angular velocity of the ro- 
bot's trunk. The assignee earlier proposed a techni- 
que for enabling such control in Japanese Laid-open 
Patent Publication No. 4(1992)-201,187 (which was 
filed in the United States on Nov. 27, 1991 under the 
number of 07/799,367 and filed in the EPO on the 
same date under the number of 91310964.1). In the 
proposed system, a number of candidate footfall pos- 
itions are established beforehand, one of the candi- 
dates is selected and the target walking pattern (gait) 
is switched during the control cycles of each walking 
step. This control has the following disadvantage. 
Once ground contactability has been diminished ow- 
ing to attitude destabilization which causes the ZMP 
(zero moment point; the center of gravity at which 
only the vertical ground reaction force acts) to shift to 
near the limit of the range within which it can exist, it 
cannot therefore be promptly restored because the 
ZMP remains near its limit until the attitude is restored 
by correcting the walking pattern in the next step. 

The object of the invention is therefore to provide 
an attitude stabilization control system for a legged 
mobile robot which enables it to maintain stable atti- 
tude during walking more effectively. 

More specifically, for eliminating the drawback of 
the prior art, the assignee recently proposed an im- 
proved control in a Japanese Patent Application filed 
on April 30, 1992 under the number of Nos. 4(1992)- 
137,884 to 137,886, one of which is being filed in the 
United States. In this technique, a target walking pat- 
tern (gait) is generated which defines the ground re- 
action force so as to ensure a state of dynamic equi- 
librium based on the robot's dynamic model and when 
an error arises between the target and actual values 
of the inclinatory angle and the angular velocity ow- 
ing to a modeling error or disturbance, a restoration 
force acting to bring the inclination of the actual robot 
closer to that of the model is produced by deliberately 
shifting a ground contact portion (foot) or moment of 
the actual robot away from that of the target walking 
pattern, thereby shifting the ground reaction force of 
the actual robot away from that of the target walking 
pattern calculated from the model. (The term "ground 



reaction force" is used here to mean the resultant 
force and moment at a point of action obtained as the 
vectorial sum of all ground reaction forces acting on 
the individual legs.) 

s When this technique is used, however, it some- 

times happens, during a one-leg support period of a 
biped walking robotfor example, that even if the ankle 
joint angle is shifted greatly for deliberately producing 
a large shift in the ground reaction force of the actual 

10 robot, it is nevertheless impossible to produce a de- 
sired ground reaction force with the ZMP outside the 
ground contact surface and, as a result, a part of the 
foot rises off the ground. Thus, when the actual robot 
has come to incline greatly with respect to the attitude 

15 defined in the model, it is not possible to obtain a 
righting force strong enough to restore the actual ro- 
bot's inclination to normal. The robot therefore falls 
over. In other words, in this system the restoring ef- 
fect for reducing the error between the target and ac- 

20 tual values is limited to within the range in which the 
ZMP can exist. 

This will be explained with reference to the draw- 
ings. In controlling the joint displacements of the ac- 
tual robot to faithfully follow that defined by the mod- 

25 el, if the inclinatory angle and inclinatory angular ve- 
locity of the actual robot agree with those of the mod- 
el, the ZMP of the actual robot drawn in solid lines in 
Figure 1 will coincide with the target ZMP determined 
from the model and the robot will walk in the desired 

30 attitude. If, however, a modeling error or a disturbance 
causes errors to arise in the robot's inclinatory angle 
and inclinatory angular velocity as indicated in broken 
lines in Figure 1, the gravitational force produces a 
tendency toward divergence in which the errors in- 

35 crease more rapidly as they grow in magnitude. If the 
actual ZMP shifts away from the target ZMP, on the 
other hand, a moment M equal to the product of the 
distance x between the actual and target ZMPs and 
the force F is produced about the target ZMP as 

40 shown in Figure 2. This means that a desired moment 
can be produced by controlling the joint displace- 
ments for deliberately shifting the actual ZMP position 
away from the target ZMP position. Based on this 
concept, the earlier proposed technique responded to 

45 the error in robot s inclinatory angle or angular veloc- 
ity by deliberately shifting the actual ZMP position as 
shown in Figure 2 so as to produce a moment in the 
direction of attitude restoration. As is clearfrom Figure 
2, however, the range within which the actual ZMP 

50 position can be shifted is limited to the foot region 
Xsole. 

Therefore, the object of this invention is, more 
specifically, to provide an attitude stabilization control 
system for a legged mobile robot which is an effective 
55 improvement on the technique recently proposed by 
the assignee. 

For realizing this, the invention provides an im- 
proved model-based attitude stabilization control sys- 
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tern for a legged mobile robot having at leastone mod- 
el which the control follows, wherein the improvement 
comprises that a manipulated variable which is a 
function of a state error such as that between the in- 
clination of the model and the actual robot is fed back 
to at least the model for modifying its behavior. 

To be specific, the action which works to con- 
verge to zero the inclinatory error which the gravita- 
tional force works to diverge is produced by the dif- 
ference between the ground reaction forces of the ac- 
tual robot and the model. Therefore, it should be pos- 
sible to obtain an effect equivalent to that produced 
by shifting the ground reaction force of the actual ro- 
bot, by instead shifting the ground reaction force of 
the model to the direction in which the attitude is re- 
stored. The present invention uses this effect As 
shown in Figure 3, since it is the ZMP of the model 
that is shifted, the range within which the shift can be 
made is no longer restricted to the region Xsole direct- 
ly under the foot but is extended far beyond the foot 
ground contact surface. This means that the afore- 
said distance x can be increased and that, as a result 
the moment can be increased to obtain a large atti- 
tude restoring force. 

This will be explained further with respect to the 
simple robot shown in Figure 4, which has only a foot 
a single-link leg and a trunk. The mass of the leg is 
designated m, the inertial moment of the leg about 
the ankle [, and the distance between the center of 
gravity of the leg and the ankle h. The mass and in- 
ertial moment of the trunk are both assumed to be 0. 
The height of the ankle from the ground is also as- 
sumed to be 0 (for simplifying the analysis by making 
the ground reaction force and the ankle moment co- 
incident). The leg's inclinatory angle relative to the 
vertical (angle of inclination of the line segment con- 
necting the ankle and the center of gravity) is desig- 
nated Blink, the trunk's inclinatory angle relative to 
the vertical 8trunk, and the hip joint's bending angle 
0hip. In the drawing, Glinkand Gtrunk are negative and 
Ghip positive. The moment acting on the ankle is des- 
ignated Mact. (Since the height of the ankle from the 
ground is 0, the ground reaction force is also Mact.) 
Gravitational acceleration is designated g. It is as- 
sumed that the ankle is subject to a conventional tor- 
que control and that the hip joint's bending angle is 
displacement controlled. 

The geometric relationship will lead Eq. 1. 
etrunk = eiink + 9hip Eq. 1 

The equation of motion of the robot's leg will be 
expressed as Eq. 2. 

I d2 ^j, nk - m g h - Blink = Mact Eq. 2 
dt 2 

Laplace transformation of Eq. 2 gives Eq. 3. 
Mact 



Blink = 



Eq. 3 



IS 2 - m g h 

Let us now create a mathematical model having 
exactly the same structure and parameters as the ac- 



tual robot. Calling the leg's inclinatory angle relative 
to the vertical (inclination of the line segment con- 
necting the ankle and the center of gravity) 8mdllink, 

5 the trunk's inclinatory angle relative to the vertical 
Omdltrunk, the hip joint's bending angle Gmdlhip, and 
the moment acting on the model ankle (i.e. the mod- 
el's ground reaction force) Mmdl, the geometric and 
the dynamics of the model will be expressed as Eq.4 

10 and Eq. 5. 

Gmdltrunk = Omdllink + Gmdlhip Eq. 4 

I . ^fink . mg h . omdllink = Mmdl Eq. 5 
dt 2 

Now let us generate a target walking pattern (gait) 

15 based on the model. The target walking pattern is 
generally expressed in terms of the initial state of the 
model, manipulated variables to be applied to the 
model and constraint conditions. Specifically, it is ex- 
pressed in terms of the leg's initial inclinatory angle 

20 GreflinkO and leg's initial inclinatory angular velocity 
omega reflinkO, the ankle moment's time function 
Mref(t), and the trunk's inclinatory angle's time func- 
tion Oreftrunk(t). To simplify the analysis, the trunk's 
inclinatory angle's time function Greftrunk(t) is as- 

25 sumed to be 0 at all times. 

Now implement a control as follows. 
The model trunk's inclinatory angle Gmdltrunk is 
maintained constantly coincident with Greftrunk(t) 
(since Greftrunk(t) is 0, Gmdltrunk is 0 at all times). 

30 The model's ankle is subjected to a torque control so 
as to maintain the sum of Mref(t) and a manipulated 
variable Mactfb (a function of the trunk's inclinatory 
angular error to be explained later) coincident with 
the actual moment Mact. The actual robot's hip joint's 

35 bending angle Gacthip is subjected to a displacement 
control using the model's hip joint's bending angle 
Gmdlhip as the target displacement. In other words, 
Gacthip is controlled to coincide with Gmdlhip. The 
trunk is equipped with an inclination sensorfor detect- 

40 ing the trunk's inclinatory angle Gacttrunk. 

The actual robot's trunk's inclinatory angular er- 
ror relative to the model Gerr is calculated using Eq. 
6. In the example at hand, since Gmdltrunk is main- 
tained constantly 0, it follows that the actual robot's 

45 trunk's inclinatory angular error Gerr is equal to Gact- 
trunk. 

Gerr = Gacttrunk - Gmdltrunk Eq. 6 
The object of the control is to converge the 
trunk's inclinatory angular error Gerr of the actual ro- 

50 bot relative to the robot model to zero during walking. 
In the aforesaid control proposed earlier by the as- 
signee, a manipulated variable Mactfb calculated as 
a function of the trunk's inclinatory angular error was 
added to the actual robot's ankle moment Mact to 

55 thereby establish a feedback loop. In contrast, in the 
system according to the present invention, a manipu- 
lated variable Mmdlfb calculated as a function of the 
trunk's inclinatory angular error is added to the mod- 
el's ankle moment Mmdl to establish a feedback loop. 
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The effect of the control according to this inven- 
tion is compared with that according to the earlier 
proposed technique in the block diagram of Figure 5 
showing from Mactfb and Mmdlfb to the inclinatory 
angular error 9err. (Note that the figure does not 
show the feedback from 8err to Mactfb or the feed- 
back from Gerr to Mmdlfb.) 

Insofar as the control response is adequately 
high relative to the behavior of Gactlink, the transfer 
functions of the torque control section and the dis- 
placement control section can be deemed to be unity. 
Assuming this to be the case and focusing solely on 
the relationship among Mactfb, Mmdlfb and Gerr, Fig- 
ure 5 can be modified as shown in Figure 6. From Fig- 
ure 6 it can be seen that if a rule, hereinafter referred 
to as "H (S)\ for example, is defined as the feedback 
rule, then feeding back Mactfb calculated according 
to Eq. 7 to Mact and feeding back Mmdlfb calculated 
according to Eq. 8 to Mact have the same effect as 
regards the behavior of Gerr. 

Mactfb = H (S) • Gerr Eq. 7 
Mmdlfb = - H (S) - Gerr Eq. 8 

The behavior of Gerr is also determined by the 
difference between Mactfb and Mmdlfb. Thus, If H (S) 
is distributed to Mactfb and Mmdlfb as shown in Eq. 
9, this also has the same effect. 

Mactfb - Mmdlfb = H(S) ■ Gerr Eq. 9 

The foregoing is the basic principle of the inven- 
tion. Insofar as a model which faithfully simulates the 
dynamics of the actual robot is created and the differ- 
ence between the ground reaction force of the actual 
robot and that of the robot model is controlled, the 
same principle can be applied with the same effect ir- 
respective of the complexity of the actual robot. As 
will be explained later, even when the dynamic model 
is expressed in terms of the perturbation from the tar- 
get walking pattern the same effect as that proposed 
in the earlier technique will be obtained insofar as the 
error Gerr and its differential omega err are small. 

Since, differently from the earlier proposed con- 
trol, the configuration according to this invention does 
not require the ZMP of the actual robot to be deliber- 
ately shifted from its proper position, even if an exter- 
nal disturbance produces a very large error in the in- 
clinatory angle and angular velocity, it achieves an at- 
titude restoration of a large magnitude not obtainable 
by the earlier proposed control and thus enables the 
error to be converged on zero in a short period of time, 
without loss of ground contactability (rising of the sole 
of the foot off the ground). Moreover, since there is 
no interference between the model and the ground 
(other than that deliberately produced for shifting the 
overall ground reaction force of the actual robot), the 
actual robot, which follows the model, does not exper- 
ience any interference with the ground either. The 
footfall impact can therefore be controlled to a low lev- 
el. 

For ensuring that the foot does not rise off the 



ground in the foregoing control, it is necessary to con- 
trol Mact (Mref + Mactfb in the foregoing example) to 
within a certain range (in level-ground walking, for ex- 
5 ample, to control it such that the actual robot's ZMP 
does not move beyond the foot contact surface). 
Thus Mactfb and Mmdlfb should be determined such 
that the value of Mact does not exceed the limit 
placed thereon and, also, so as to satisfy Eq. 10. 
10 Mactfb - Mmdlfb = H (S) Gerr Eq. 10 

It should further be noted that, since the open 
loop's transfer function between Mactfb and Gmdllink 
and the open loop's transfer function between Mmdlfb 
and Gmdllink are not the same, the control according 
15 to the earlier proposed technique and the control ac- 
cording to this invention exhibit different characteris- 
tics as regards the behavior of Gmdllink. 

The above and other objects and advantages of 
the invention will be more apparent from the following 
20 description and drawings, given by way of example 
only, in which: 

Figure 1 is a view explaining the control proposed 
earlier by the assignee of the invention using a 
concept of ZMP (zero moment point; the center of 
25 gravity at which only the vertical ground reaction 

force acts) in which a mathematical robot model 
is established to define a target position of the 
ZMP and an actual robot is controlled such that 
its actual ZMP is coincides with the target posi- 
30 tion; 

Figure 2 is a view showing a moment resulting 
when the actual ZMP shifts from the target ZMP 
position which can be used to restore robot's at- 
titude, if destabilized, by deliberately producing it; 
35 Figure 3 is a view, similar to Figure 2, but showing 

the basic principle of the invention; 
Figure 4 is a block diagram explaining a control 
based on the basic principle of the invention tak- 
ing a simple robot structure as an example; 
40 Figure 5 is a block diagram explaining the control 

of Figure 4 in more detail; 
Figure 6 is a block diagram rewritten from that of 
Figure 5 in a simple form; 

Figure 7 is an overall skeleton view of an attitude 
45 stabilization control system for a legged mobile 

robot according to the invention; 
Figure 8 is a block diagram showing a detailed 
structure of a control unit illustrated in Figure 7; 
Figure 9 is a block diagram showing the control 
so according to a first embodiment of the invention; 

Figure 10 is a flow chart showing a main routine 
of the control of Figure 9; 
Figure 11 is a flow chart showing a subroutine of 
attitude control procedures of Figure 10; 
55 Figure 1 2 is a flow chart showing a subroutine of 

compliance control procedures of Figure 11; 
Figure 13 is a block diagram showing the control 
according to a second embodiment of the inven- 
tion; 
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Figure 14 is a block diagram showing a limiter and 
a distributor used in the second embodiment and 
illustrated in Figure 13; 

Figure 15 is a flow chart showing a main routine 5 

of the control of Figure 13; 

Figure 16 is a flow chart showing a subroutine of 

attitude control procedures of Figure 15; 

Figure 17 is a flow chart showing a subroutine of 

compliance control procedures of Figure 16; 10 

Figure 18 is a block diagram showing the control 

according to a third embodiment of the invention; 

Figure 19 is a flow chart showing a main routine 

of the control of Figure 18; 

Figure 20 is a flow chart showing a subroutine of 15 

attitude control procedures of Figure 19; 

Figure 21 is a flow chart showing a subroutine of 

compliance control procedures of Figure 20; 

Figure 22 is a block diagram showing the control 

according to a fourth embodiment of the inven- 20 

tion; 

Figure 23 is a flow chart showing a main routine 
of the control of Figure 22; 
Figure 24 is a flow chart showing a subroutine of 
attitude control procedures of Figure 23; 25 
Figure 25 is a flow chart showing a subroutine of 
compliance control procedures of Figure 24; 
Figure 26 is a block diagram showing the control 
according to a fifth embodiment of the invention; 
Figure 27 is a flow chart showing a main routine 30 
of the control of Figure 26; 
Figure 28 is a flow chart showing a subroutine of 
attitude control procedures of Figure 27; 
Figure 29 is a flow chart showing a subroutine of 
compliance control procedures of Figure 28; 35 
Figure 30 is a block diagram showing the control 
according to a sixth embodiment of the invention; 
Figure 31 is a flow chart showing a main routine 
of the control of Figure 30; 

Figure 32 is a flow chart showing a subroutine of 40 
attitude control procedures of Figure 31 ; and 
Figure 33 is a flow chart showing a subroutine of 
compliance control procedures of Figure 32. 
Embodiments of the invention will now be ex- 
plained taking a biped robot as an example of a leg- 45 
ged mobile robot. 

Figure 7 is an overall skeleton view of an attitude 
stabilization control system for a legged mobile robot 
according to the invention. In the figure, a robot 1 has 
left and right legs 2 each having six joints (axes). (To 
make the arrangement easier to understand, the 
joints (axes) are represented as the electric motors by 
which they are driven.) The six joints (axes) are, start- 
ing at the top, joints (axes) 10R, 10L for swiveling 
(generally horizontal rotation) of the legs at the hip (R 
and L indicating the right and left legs), joints (axes) 
12R, 12Lfor rotation at the hip in the pitch direction 
(rotation about the x axis), joints (axes) 14R, 14L for 
rotation at the hip in the roll direction (rotation about 



the y axis), joints (axes) 16R, 16L for rotation at the 
knee in the roll direction, joints (axes) 18R, 18Lfor ro- 
tation atthe ankle in the roll direction and joints (axes) 
20R, 20L for rotation atthe ankle in the pitch direction. 
Feet 22R, 22L are provided at the lower end of this 
arrangement and a trunk (body) 24 is provided at the 
upper end, which houses electronic components such 
as a control unit 26 made up of a microcomputer. 

The hip joints in the foregoing configuration are 
constituted by the joints (axes) 10R(L), 12R(L) and 
14R(L) and the ankle joints by the joints (axes) 18R(L) 
and 20R(L). The hip and knee joints are connected by 
thigh links 32R, 32L and the knee joints and ankle 
joints by crus links 34R, 34L. The legs 2 thus have six 
degrees of freedom, so that during locomotion the 
legs as a whole can be caused to execute the desired 
motion by driving the 6x2= 12 joints (axes) to ap- 
propriate angle. The robot is thus capable of walking 
freely within three dimensional space. The joints are 
provided mainly with electric motors, as was men- 
tioned earlier, and reduction gear mechanism for in- 
creasing motor torque. The structure of the joints is 
described in the assignee's earlier Japanese Patent 
Application No. 1(1989)-324,218 (Japanese Laid- 
Open Patent Publication No. 3(1 991)- 184,782) etc., 
and since it is not an essential aspect of the present 
invention, will not be explained further here. 

The individual ankles of the robot 1 shown in Fig- 
ure 7 are provided with a six dimensional force and 
torque sensor 36 of conventional design. By measur- 
ing the x, y and z force components Fx, Fy and Fz 
transmitted to the robot through the feet and also 
measuring the moment components Mx, My and Mz 
around the three axes, the six-dimensional force and 
torque sensor 36 detects whether or not the associ- 
ated foot has landed and the magnitude and direction 
of the forces acting on the supporting leg. The sole 
of each foot is equipped at its four corners with touch- 
down switches 38, not illustrated in Figure 7, of con- 
ventional design for detecting whether or not the foot 
is in contact with the ground. The top of the trunk 24 
is provided with an inclination sensor 40 for detecting 
the robot's inclinatory angle and angular velocity rel- 
ative to z axis in the x-z and y-z planes. Each electric 
motor at the individual joints is provided with a rotary 
encoder for generating rotational information. Also, 
although not illustrated in Figure 7, the robot 1 is pro- 
vided with a zero reference switch 42 for calibrating 
the output of the inclination sensor 40 and a limit 
switch 44 for a fail safe. The outputs of the sensors 
36 and the like are sent to the control unit 26 in the 
trunk 24. 

As shown in the block diagram of Figure 8, the 
control unit 26 has a microcomputer. The outputs from 
the inclination sensor 40 etc. are converted into digi- 
tal signals by an A/D converter 50 and the resulting 
digital values are sent via a bus 52 to a RAM (random 
access memory) 54 for storage. In addition, the out- 
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puts of encoders disposed adjacent to the respective 
motors are input to the RAM 54 through a counter 56, 
while outputs of the touchdown switches 38 and the 
like are stored in the RAM 54 via a waveform shaper 5 
58. The control unit has a first processor 60, a second 
processor 62 and a ROM 64. The first processor 60 
computes target joint angles on the basis of a walking 
pattern (gait) and outputs the same to the RAM 54. 
The second processor 62 fetches the target joints an- 10 
gles and measured joint angles from the RAM 54, 
computes control commands of the individual joint 
motors and sends the same to associated servo am- 
plifiers thereof via a D/A converter 66. 

The operation of the control system will now be 15 
explained in detail. 

Figure 9 is a block diagram showing the operation 
of the control system according to a first embodiment 
of the invention and Figures 10 to 12 are flow charts 
showing the same. Before going into a detailed ex- 20 
planation of the control with reference to Figures 10 
to 12, the control will first be explained generally with 
reference to Figure 9. While the following explanation 
will be made only in respect of the x direction (direc- 
tion of robot advance), the same also applies to the 25 
y direction (lateral direction thereto). 

As shown in Figure 9, in the present embodiment 
a rigid model is used as the geometric model of the 
robot, while as the dynamic model there is used one 
which simulates the behavior of the trunk's positional 30 
perturbation (variation) component relative to a target 
walking pattern as an inverted pendulum. Similarly to 
those mentioned earlier, the mass of the inverted 
pendulum is defined as m, the inertial moment about 
the pivot point as [and the distance between the pivot 35 
point and the center of gravity as h. The target walking 
pattern is expressed in terms of the position/orienta- 
tion of the trunk 24 and the positions/orientations of 
the feet 22R (L). As regards the positions/orientations 
of the feet 22R (L), for each walking step, foot trajec- 40 
tory's parameters (e.g., the free leg's footfall position, 
footfall time etc.) are supplied from a macro-attitude 
controller to a target foot trajectory generator which 
generates the instant-by-instant positions and orien- 
tations of the individual feet in real time. If the robot's 45 
attitude should destabilize in the course of this con- 
trol, the attitude of the model is first similarly desta- 
bilized and then restored toward stabilization. The ac- 
tual robot is simultaneously controlled in line with the 
model so as to restore its attitude stability. Thus, the 50 
attitude restoration is not conducted instant by instant 
but is achieved smoothly over a relative long period 
as the final result. The term "macro- attitude" is used 
here to indicate this characteristic of the control. 

On the other hand, target ZMP trajectory's para- 55 
meters for each walking step (in the case where they 
are expressed by broken-line curves, for example, 
the coordinates and times of the broken-line curve 
joints) are supplied from the macro-attitude controller 



to a target ZMP trajectory generator which calculates 
the target position of the ZMP at each instant in real 
time. And the position/orientation of the trunk 24 is 
controlled by storing time series data generated in ad- 
vance offline in the macro-attitude controller so as to 
satisfy the targetZMP trajectory and causing the data 
to be output during walking. (The foot trajectory's 
parameters, target ZMP trajectory's parameters and 
other parameters expressing the characteristics of 
the walking pattern will be referred to generally as the 
"walking pattern's parameters" in the following.) 

Next, the inclinatory angular error Gerr and the 
angular velocity error omega err of the actual robot 
relative to the geometric model are detected. Here, if 
it can be assumed that the joint displacement of the 
actual robot 1 follows those of the geometric model 
faithfully, the inclinatory angle and angular velocity of 
the trunk 24 can be used to represent the attitude's 
inclinatory angle and angular velocity. It is also pos- 
sible to use a positional error of the center of gravity. 

These are defined as the state quantities and the 
manipulated variable Mmdl for converging them on 
zero (state feedback amount) obtained from an actual 
robot's inclinatory error control's feedback rule is de- 
termined in terms of moment and applied to the pivot 
point of the inverted pendulum type dynamic model. 
The manipulated variable is, hereinafter, called the 
moment Mmdl for manipulating model. The moment 
Mmdl can be said to correspond to the ground reac- 
tion force moment acted on the dynamic model. The 
inverted pendulum type dynamic model moves under 
the influence of Mmdl and the gravitational force act- 
ing on its center of gravity. The state quantities of the 
dynamic model, namely the behaviors of the inclina- 
tory angle 8mdl and its angular velocity omega mdl 
(omitted from illustration), are calculated successively 
based on the past states and the moment Mmdl ap- 
plied to the pivot point. 

The positional error delta x of the trunk 24 in the 
x direction is calculated using Equation 11. 

Ax = Omdl ■ htrunk Eq. 11 
where htrunk is the height of the trunk 24 (more pre- 
cisely the height of the hip joint above the ground). 

The geometric model's trunk position is designat- 
ed as the position obtained by adding delta x to an ini- 
tial trunk position x of the walking pattern's parame- 
ter. 

In addition, the compliance control employed in 
the earlier proposed technique is used to correct the 
target walking pattern's feet positions/orientations in 
accordance with the actual ground reaction force mo- 
ment Mact around the target ZMP position, and the 
corrected position and orientation are applied to the 
geometric model. In the control shown in Figure 9, a 
virtual ground's inclinatory angular command Octrl is 
calculated from Mact using a ground reaction force 
moment control's feedback rule and the model's feet 
positions are rotated from that generated by the tar- 
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get foot trajectory generator about the target ZMP 
position by the amount of the virtual ground's inclina- 
tory angular command Gctrl. (Here, the virtual 
ground's inclinatory angular command Gctrl is an an- 
gle as if the ground were inclined by the angle such 
that the robot legs (feet) are rotated by the amount to 
produce the aforesaid moment for the attitude restor- 
ation.) As a result, the inclinatory behavior of the ac- 
tual robot 1 is simulated by an inverted pendulum 
which produces a ground reaction force moment pro- 
portional to the virtual ground's inclination abouta ref- 
erence point (e.g., the target ZMP position). 

The displacements of the individual joints of the 
geometric model are obtained by inverse kinematic 
calculation from the trunk's position/orientation calcu- 
lated and corrected in the foregoing manner. Since 
the movable portions of the robot have only rotational 
freedom, ail joint displacements are expressed in an- 
gles. The robot 1 is equipped with the aforesaid servo 
control system for causing its joint displacements to 
follow the joint displacements of the geometric model. 

In the foregoing system, the moment Mmdl for 
manipulating model which is a function of the state 
quantities Gerr and omega err is fed back to the dy- 
namic model and this feedback control acts to con- 
verge Gerr and omega err on zero in accordance with 
the basic principle explained earlier. In other words, 
it acts to bring the actual robot and the model into co- 
incidence in the inclination. 

In Figure 9, once the application of the moment 
Mmdl for manipulating model to the dynamic model 
causes its inclinatory angle Gmdl and angular velocity 
omega mdl to shift away from the neutral point (up- 
right state), the dynamic model becomes instable and 
hence 9mdl and omega mdl diverge as a result The 
divergence of Gmdl and omega mdl (omitted from illus- 
tration) is therefore controlled by varying the next free 
leg's footfall position or footfall time in accordance 
with the state quantities Gmdl and omega mdl. 

Here, if the footfall position is changed during a 
given walking step of the target walking pattern (re- 
ferred to as the "first walking pattern" hereinafter), 
only the supporting leg's position will, during the first 
part of the next walking step (also referred to as the 
"second walking pattern"), be shifted away from the 
position of the target walking pattern. From the view- 
point of reference coordinates of the second walking 
pattern, this corresponds to the supporting leg's foot 
position being maintained that of the target walking 
pattern, with only the positions of the free foot and the 
trunk being shifted by the amount of the variation. Un- 
less some countermeasure is taken, the boundary 
between the first and second walking patterns will 
thus become discontinuous. For ensuring the con- 
tinuity of the geometric model's walking pattern, 
therefore, the following operation is conducted at the 
boundary between the walking patterns. 

Since the free leg's trajectory is defined by the 



foot trajectory's parameters such as the initial posi- 
tion and the next footfall position, the free leg's initial 
position which is one foot trajectory's parameter of 

5 the second walking pattern is shifted by the amount 
of minus delta xfoottotal. The trunk trajectory of the 
second walking pattern is maintained as it is but the 
inclinatory angle Gmdl of the dynamic model is shifted 
by minus delta xfoottotal/htrunk. As a result, it be- 

10 comes possible to manipulate the inclinatory angle 
Gmdl, which is one of the state quantities of the dy- 
namic model, by the amount of minus xfoottotal- 
/htrunk, while maintaining the continuity of the geo- 
metric model's walking pattern. In other words, it be- 

15 comes possible to control the state quantity of the dy- 
namic model by changing the footfall position. 

Based on the foregoing, an explanation will now 
be made with reference to the flow chart of Figure 1 0 
regarding the model's inclination control algorithm for 

20 the case where the footfall position is changed in the 
direction of robot advance. 

First, in stepSIO, the dynamic model is initialized 
to a stable upright position. Control then passes to 
step S12 in which steps S14 to S26 are repeated until 

25 a count of the walking step counter reaches a target 
number of walking steps defined by the target walking 
pattern. The following terms are defined to have the 
following meanings: 

1 . "Walking pattern of one walking step" is de- 
30 fined as the period between the beginning of a 

two-leg support period and the next free leg's 
footfall. 

2. "The supporting leg in a certain walking pat- 
tern" is defined as the leg which provides support 

35 during the one-leg support period of the walking 

pattern, and "the free leg" is defined as the leg 
which is free during the one-leg support period. 

3. "The reference coordinates of the walking pat- 
tern" are defined as those of the ground contact 

40 point of the foot of the supporting leg during the 

walking step concerned. 

The walking pattern's parameters for the current 
cycle are set in step S14, whereafter control passes 
to step S16 in which the difference between the cor- 
45 rected footfall position and the initial footfall position 
of the target walking pattern in the preceding cycle is 
defined as delta xfoottotal. Control then passes to 
step S18 in which delta xfoottotal is subtracted from 
the free leg's initial target position of the walking pat- 
so tern in the current cycle and to step S20 in which delta 
xfoottotal/htrunk is subtracted from the inclinatory an- 
gle Gmdl of the dynamic model. 

In the following step S22, steps S24 and S26 are 
repeated by the number of sampling times for one 
55 walking step. More specifically, after the elapse of 
one cycle has been ascertained in step S24, control 
passes to step S26 in which an attitude control sub- 
routine is executed. 

The subroutine is shown in Figure 11. First, in 
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step S1 00, the inclination of the actual robot is meas- 
ured and, as explained above, the actual robot's incli- 
natory angular error Oerr and the angular velocity er- 
ror omega err relative to the geometric model are de- 
termined. Control then passes to step S102 in which 
the aforesaid moment Mmdl for manipulating model 
about the target ZMP position is calculated from the 
determined values using an actual robot's inclinatory 
error control's feedback rule and to step S104 in 
which Mmdl is added to the pivot point of the dynamic 
model and its inclinatory angle Gmdl and angular ve- 
locity omega mdl in the current cycle are calculated. 
Control then passes to step S106 in which, for the 
reason explained earlier, the trunk's positional error 
delta x is calculated in the manner shown, to step 
S108 in which, provided there is time before the next 
footfall, the behavior of the dynamic model is predict- 
ed and a target footfall's positional correction amount 
delta xfoot is calculated and added to the initial target 
footfall position of the target walking pattern to cor- 
rect the same. 

Expanding on the explanation given regarding 
this earlier, the amount of change in the footfall pos- 
ition should be determined so that the inclinatory an- 
gle 9mdl and the inclinatory angular velocity omega 
mdl of the dynamic model and the inclinatory angular 
error Oerr and the inclinatory angular velocity error 
omega mdl of the actual robot relative to the geomet- 
ric model will, to the utmost possible, be converged on 
zero within the next walking step. For this it is neces- 
sary to predict from Gmdl, omega mdl, Oerr, omega err 
and other current state parameters what behavior the 
change in footfall position will induce in these Gmdl, 
omega mdl, Oerr, omega err during the next walking 
step. 

The basic principle of this invention works to con- 
verge Oerr and omega err on zero. Thus, Gerr and 
omega err can be ignored without causing any par- 
ticular problem aside form a slight degradation of pre- 
diction precision. This means that no adverse effects 
arise from determining the amount of change in the 
footfall position on the basis of a prediction calcula- 
tion based solely on the current state of Gmdl and 
omega mdl. It is also possible to map the relationship 
between the current state and the amount of change 
in the footfall position in advance in the ROM 64 by 
simulation and then use the mapped values to deter- 
mine the amount of change in footfall position during 
actual walking. 

One generally known method of determining the 
amount of change in footfall position from only the 
current states of Gmdl and omega mdl is to determine 
the amount of change so that the predicted value of 
the total energy of the dynamic model at the instant 
of footfall coincides with the total energy at the time 
of its upright standing. This method, which is com- 
monly used with biped stilt robots and the like, is ex- 
plained below. 
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For the total energy atthe instant of footfall to co- 
incide with the total energy at the time of upright 
standing, it is necessary to determine the footfall pos- 
5 ition so that the predicted values of Gmdl and omega 
mdl immediately after footfall satisfy Eq. 12. 

©mdl 2 = m g h • sin (Gmdl) • tan (Gmdl) Eq. 

12 

Since Gmdl and omega mdl have to be of opposite 
polarity in order to approach upright standing, it is 
necessary to satisfy Eq. 13. 

Gmdl • comdl £ 0 Eq. 13 
In view of Eqs. 12 and 1 3, Eq. 14 will be ob tained, 
(omdl = - ^2jtvoJi sjn (0mdI) . tan (ernd |) Eq. 
14 

Eq. 14 can be approximated as Eq. 15. 

Gmdl Eq. 15 

In other words, it suffices to determine the foot- 
fall's positional correction amount delta xfoot such 
that the ratio between omega mdl and Gmdl imme- 
diately after upright standing becomes that defined 
by Eq. 15. The predicted values of omega mdl and 
Gmdl immediately following footfall in the case where 
the footfall position is not corrected can be obtained 
by kinematic calculation from the current omega mdl, 
Gmdl, Gerr and omega err of the dynamic model, and 
are designated "omega mdle" and "Gmdle". (In partic- 
ular, if Gerr and omega err are so small in comparison 
with omega mdl and Gmdl as to be negligible, they can 
easily be calculated analytically.) Then if Gmdle can 
be corrected to omega mdle/square root of (2mgh/l), 
Eq. 15 will be satisfied. Since, as was explained ear- 
lier, Gmdl can be manipulated by the amount of minus 
delta xfoot/htrunk by correcting the footfall position 
by the amount of delta xfoot, this can be achieved by 
determining the footfall's positional correction 
amount delta xfoot in accordance with Eq. 16. 

a t - Gmdle + (omdle 
Axfoot = - . — 

\ 2 m g h / 1 

Thus the footfall's positional correction amount 
delta xfoot is determined and the target footfall posi- 
tion xfoot is corrected in the course of the walking pat- 
tern. This correction is conducted independently for 
each walking step in the case where the change in 
the target footfall position greatly complicates the foot 
trajectory generation. However, when the foot trajec- 
tory is generated using another technique onto a 
function generator recently proposed by the assignee 
in a Japanese Patent Application No. 4(1992)- 
155,919 filed on May 22, 1992 and being filed with 
the United States and the EPO, the change in the tar- 
get footfall position can be conducted every sampling 
cycle. This is because in accordance with the pro- 
posed function generator even if the target footfall 
position is changed every sampling cycle, the foot tra- 
jectory is again smoothly corrected from that point. 



htrunk Eq. 16 
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Since greatly changing the footfall position just before 
the footfall time causes an abrupt change in the free 
leg's trajectory, however, it is preferable to calculate 
the maximum possible recorrection as a function of 
the time remaining before footfall and then limit the 
rate of change in the correction amount based on this 
maximum. 

Control passes to step S110 of the flow chart of 
Figure 11 in which thetargetZMP position is calculat- 
ed from the target ZMP trajectory's parameters, to 
step S112 in which the positions/orientations of both 
feet are calculated from the target footfall position and 
other foot trajectory's parameters, to step S114 in 
which the position obtained by adding the product of 
9mdl and htrunk to the trunk's position of the target 
walking pattern is defined as the target trunk position 
(the target trunk orientation being that according to 
the target walking pattern), and to step S116 in which 
compliance control is conducted by correcting the 
moders foot. 

The subroutine for this is shown in Figure 12. 
First, in step S200, the detection value of the six-di- 
mensional force and torque sensor 36 is read in. Con- 
trol then passes to step S202 in which the actual 
ground reaction force moment Mact around the target 
ZMP position is calculated from the detected value, to 
step S204 in which the coordinate rotation angle (the 
virtual ground's inclinatory angular command) Octrl is 
calculated in the manner shown, and to step S206 in 
which the positions/orientations of both feet are rotat- 
ed about the ZMP by the calculated amount to put 
them in the positions/attitudes of the feet of the geo- 
metric model. Since this control is explained in detail 
in the aforesaid earlier Japanese Patent Application 
No. 4(1992)-137,884 filed on April 30, 1992, it will not 
be explained further here. 

Control then passes to step S118 in the flow 
chart of Figure 11 in which the joint displacements of 
the geometric model are calculated from the target 
trunk's position/orientation and the corrected foot's 
positions/orientations and to step S120 in which the 
joint displacements of the actual robot are controlled 
to follow the joint displacements of the geometric 
model. 

As is clear from the foregoing, the present em- 
bodiment achieves stable walking by conducting dis- 
crete control of the inclination of the actual robot rel- 
ative to the model and the inclinatory error of the dy- 
namic model for the individual walking steps. More- 
over, since it is the ZMP of the model that is deliber- 
ately shifted, the range over which the ZMP can be 
moved is greater than in the earlier proposed control 
and, consequently, it is possible to obtain a large at- 
titude restoration force. As a result, attitude stability 
can be restored without loss of ground contactability 
even in cases where a disturbance of some type or 
other produces a very large inclinatory error. Further, 
when the attitude becomes unstable, the attitude of 



the model is first destabilized accordingly and is 
thereafter restored, while the actual robot is control- 
led to follow the attitude of the model. As a result, the 

5 attitude restoration can be achieved by smooth mo- 
tions. In addition, since no interference arises be- 
tween the model and the ground, the actual robot 
controlled to follow the model does therefore not ex- 
perience interference with the ground either and, as 

10 a result, the footfall impact can be held to a low level. 

Figure 13 is a block diagram showing the control 
according to a second embodiment of the invention. 
While the first embodiment uses the footfall position 
as the manipulated variable Mmdl in the inclinatory 

15 control of the dynamic model, the second embodi- 
ment achieves stabilization by adding, to the Mmdl, a 
manipulated variable that is a function of the dynamic 
model's inclination. 

Further, in the second embodiment, a demand 

20 value Merrdmd obtained by passing the inclinatory 
angular error 8err (and the angular velocity omega 
err omitted from illustration in Figure 13 and on) of the 
actual robot relative to the geometric model through 
an actual robot's inclinatory error control's feedback 

25 rule H1 (S) of the PD type or the like (i.e. Merrdmd = 
H1 (S) 8err) and another demand value Mmdldmd ob- 
tained by passing the inclinatory angle 8mdl of the dy- 
namic model through a model's inclinatory error con- 
trol's feedback rule H2 (S) (i.e. Mmdldmd = H2 (S) 

30 9mdl) are passed through a limiter and a distributor 
and distributed to determine a moment Mactcomm for 
manipulating actual robot about the target ZMP pos- 
ition and the moment Mmdl for manipulating model. 
The limiter and distributor are shown in Figure 14. 

35 In this embodiment, the moment Mactcomm for ma- 
nipulating actual robot is obtained by passing the sum 
of the demand values Merrdmd and Mmdldmd 
through the limiter. This limiter ensures the ground 
contactability of the actual robot by preventing the 

40 ground reaction force (the ZMP) of the actual robot 
from moving outside the permissible region. (During 
level ground walking, the permissible region for the 
ground reaction (the ZMP) is that within the minimum 
convex polygon including the ground contact surface, 

45 as mentioned earlier.) 

The moment Mmdl for manipulating model is ob- 
tained by adding the excess of the limiter input to the 
demand value Mmdldmd and, as in the first embodi- 
ment, is input to the dynamic model. The ground re- 

50 action force moment's feedback rule obtains a ground 
reaction force moment feedback's manipulated vari- 
able 9comp from the actual ground reaction force mo- 
ment Mact about the target ZMP position. The virtual 
ground's inclination angular command 9ctrl is ob- 

55 tained by adding the ground reaction force moment 
feedback's manipulated variable Ocomp to the prod- 
uct of the moment Mactcomm and the compliance 
constant Kcomp, as will be explained with reference 
to Figure 17. As for the foot position of the geometric 
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model, the foot position of the target walking pattern 
generated by the target foot trajectory generator is 
coordinate-rotated about the target ZMP position by 
the amount of the virtual ground's inclinatory angular 
command Gctrl. 

It is the difference between the actual ground 
force moment Mact about the target ZMP position and 
the moment Mmdl for manipulating model that deter- 
mines the behavior of the actual robot's inclinatory 
angular error eerr. If the compliance control results 
adequate softness, Mact nearly coincides with the 
moment Mactcomm for manipulating actual robot. In 
such case, it can therefore be assumed that the be- 
havior of the actual robot's inclinatory angular error 
9err is the difference between Mactcomm and Mmdl. 
Since the difference between Mactcomm and Mmdl 
is always Merrdmd irrespective of whether or not the 
limiter is in operation, the control constantly functions 
to converge the actual robofs inclinatory angular er- 
ror 9err relative to the geometric model on zero. 

On the other hand, it is Mmdl that determines the 
behavior of the model's inclinatory angle Gmdl. When 
the limiter is not in operation, Mmdl equals Mmdldmd, 
and the control constantly functions to converge the 
model's inclination on zero. When the limiter is in op- 
eration, priority is given to converging the actual ro- 
bot's inclinatory angular error Gerr with respect to the 
geometric model on zero, and the model's inclination 
Gmdl is converged on zero to the extent possible. If 
the demand value Merrdmd is very large relative to 
the limiter setting, the model's inclination control is 
sacrificed and the model's inclination tends to di- 
verge. More specifically, the dynamic model's inclin- 
ation control is conducted only when the actual ro- 
bot's ZMP is well within the limits of the permissible 
region. Therefore, while the dynamic model can re- 
store the upright state against instantaneous or min- 
ute disturbances, it is unable to restore it against large 
disturbances of long duration, so that the attitude of 
the geometric model becomes very unstable and tips 
over. Since the foot trajectories are in accord with the 
target walking pattern, however, the second embodi- 
ment is highly appropriate for use when the footfall 
position is constrained, as during walking on stairs or 
stepping-stones. 

The setting of the inclination control's gain of the 
dynamic model will now be explained. The control rule 
for converging the model's inclinatory angle Gmdl on 
zero is given in the form of Eq. 17, for example. 
Mmdldmd = - Kco • comdl - KG • Gmdl Eq. 17 

A simple inverted pendulum allowed to move 
freely from its current state without application of a 
moment about its pivot point can be made to assume 
a stationary upright condition only if the current total 
energy is equal to the total energy in the upright sta- 
tionary state. In other words, Eq. 18 has to be satis- 
fied. 
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1 I comdl 2 = m g h sin (Gmdl) • tan (Gmdl) Eq. 
18 

Further, since omega mdl and Gmdl have to be of 
opposite polarity for bringing the inverted pendulum 
toward the upright state, Eq. 19 has to be satisfied. 
Gmdl - comdl ^0 Eq. 19 
From Eqs . 18 and 19, Eq. 20 will be obtai ned. 

comdl = - V^— 9 -^ • sin (Gmdl) tan (Gmdl) Eq. 
20 

Eq. 20 can be approximated as Eq. 21. 

comdl = - V 2m ^ h • Gmdl Eq. 21 

If the state satisfies Eq. 21, the dynamic model 
assumes a stationary upright state even without ap- 
plying the moment Mmdl about the pivot point. There- 
fore, if the control rule is established so that the de- 
mand value Mmdldmd becomes zero in the state sat- 
20 isfying Eq. 21 , it becomes possible to converge on the 
upright state without producing the moment Mmdl in 
the dynamic model any more often than necessary. 
The control rule of this type which satisfies Eq. 17 will 
be expressed as Eq. 22. 

25 Mmdldmd = - Kco • comdl - Kco • V 2 m 9 h • Gmdl 

Eq. 22 

where K omega is an arbitrary positive constant 

Even if Eq. 22 is satisfied, the inverted pendulum 
model will exhibit overshoot behavior if K omega is set 
somewhat small. If K omega is set smaller still, the 
gravitational force moment prevails and restoration to 
the upright state becomes impossible. For ensuring 
restoration to the upright state without giving rise to 
overshooting, the characteristic root of the feedback 
loop has to be a negative real number, which requires 
K omega to satisfy Eq. 23. 

Kco ^ (2 + 2^2) Vmgh I Eq. 23 
If the gain is set to satisfy Eqs. 22 and 23, the de- 
40 mand value Mmdldmd is held to a small absolute val- 
ue so that the moment Mactcomm is held to a small 
absolute value. The ground contactability of the ac- 
tual robot is thus enhanced. 

Figures 1 5 to 1 7 are flow charts showing the con- 
45 trol according to the second embodiment of the inven- 
tion. Steps S300 to S310 of Figure 15 make up the 
main routine, steps S400 to S422 of Figure 16 a sub- 
routine of the attitude control procedures of the main 
routine, and steps S500 to S506 of Figure 17 a sub- 
50 routine of the compliance control procedures of the 
main routine. The explanation will focus on the points 
of the second embodiment that distinguish it from the 
first. Steps S304 to S306 of the flow chart of Figure 
15 include nothing corresponding to steps S1 6 to S20 
55 of Figure 1 0 of the first embodiment. This is because 
the attitude control subroutine does not conduct foot- 
fall's positional correction. Moreover, comparing the 
attitude control subroutine flow chart of Figure 1 6 with 
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the flow chart of Figure 11, in light of the configuration 
of the second embodiment described in the forego- 
ing, the flow chart of Figure 16 additionally includes 
steps S402 to S406 but does not include a step cor- 
responding to S108 of the first embodiment. On the 
other hand, in the subroutine flow chart of Figure 17, 
the coordinate rotation angle computation of step 
S504 involves the aforesaid operation of adding to 
the coordinate rotation angle the product of the mo- 
ment Mactcomm and the compliance constant 
Kcomp. The remainder of the configuration is the 
same as that of the first embodiment 

The fact that its foot trajectory remains un- 
changed from that of the target walking pattern 
makes the second embodiment preferable for use in 
circumstances where the footfall position is con- 
strained, as during walking on stairs or stepping-sto- 
nes. 

Figure 18 is a block diagram showing the control 
according to a third embodiment of the invention. The 
third embodiment combines the methods of the first 
and second embodiments as regards the inclination 
control of the dynamic model. Here, in restoring the 
dynamic model to the upright state, as against instan- 
taneous and minute disturbances, the main role is 
played by the control achieved by applying the mo- 
ment Mmdl to the dynamic model, while as against 
large disturbances of long duration, the main role is 
played by the control achieved by correcting the foot- 
fall's position and time. 

As in the first embodiment, the footfall's position- 
al and time correctional amounts are determined in 
the third embodiment so as to cause the dynamic 
model to return to the upright state. While it would ap- 
pear that the model behavior's prediction would be 
more complicated than in the first embodiment be- 
cause of the effect of the demand value Mmdldmd, 
the fact that the demand value Mmdldmd suppresses 
divergence of the dynamic model actually makes the 
prediction's accuracy higher. In particular, if the dy- 
namic model's inclination control's gain setting of the 
second embodiment (gain setting satisfying Eqs. 22 
and 23) is adopted, the demand value Mmdldmd be- 
comes zero when the total energy of the dynamic 
model is equal to the total energy at the time of upright 
standing, which makes it possible to adopt without 
modification the method explained with respect to 
the first embodiment of determining the amount of 
change so that the predicted value of the total energy 
of the dynamic model at the instant of footfall coin- 
cides with the total energy at the time of upright 
standing. 

Steps S600 to S616 of Figure 19 make up the 
main routine, steps S700 to S724 of Figure 20 a sub- 
routine of the attitude control procedures of the main 
routine, and steps S800 to S806 of Figure 21 a sub- 
routine of the compliance control procedures of the 
main routine. The main routine flow chart of Figure 1 9 



is the same as the corresponding flow chart of the 
first embodiment and the subroutine flow chart of Fig- 
ure 21 is the same as the corresponding flow chart 

5 of the second embodiment. The attitude control sub- 
routine flow chart of Figure 20 differs from the corre- 
sponding flow chart of the second embodiment only 
in that it includes a step S712for correction of the tar- 
get footfall position. The remainder of the conf igura- 

10 tion is the same as that of the earlier embodiments. 

As explained earlier, in the third embodiment re- 
storation of the dynamic model to the upright state 
against instantaneous and minute disturbances relies 
mainly on the control achieved by applying the mo- 
ts ment Mmdl to the dynamic model, while restoration 
thereof against large disturbances of long duration re- 
lies mainly on the control achieved by correcting the 
footfall's position and time. As a result, the amount of 
footfall's positional correction and, accordingly, the 

20 amount of stagger become smaller than in the first 
embodiment, while stability is enhanced over both 
the first and second embodiments. 

Figure 22 is a block diagram showing the control 
according to a fourth embodiment of the invention. 

25 The first to third embodiments are each provided sep- 
arately with the dynamic model for the trunk's posi- 
tional perturbation component relative to the target 
walking pattern and with the geometric model ex- 
pressing the relationship of the trunk's and foot's pos- 

30 itions to the joint angles. In contrast, the fourth em- 
bodiment is provided with a composite dynamic and 
geometric model comprising of rigid links. In other re- 
spects, aside from the specifics of the macroattitude 
controller to be explained below, its configuration is 

35 substantially the same as that of the first embodi- 
ment. 

In the fourth embodiment, the walking pattern is 
expressed in terms of the target 2MP position and the 
positions/orientations of both feet. As regards the 

40 positions/orientations of the feet, for each walking 
step, foot trajectory's parameters (e.g., the free leg's 
footfall position, footfall time etc.) are supplied from 
the macroattitude controller to the target foot trajec- 
tory generator which generates the instant-by-instant 

45 positions and orientations of the individual feet in real 
time. As regards the target ZMP trajectory, for each 
walking step, the target ZMP trajectory's parameters 
are supplied from the macro- attitude controller to the 
target ZMP trajectory generator which calculates the 

so instant-by-instant position of the target ZMP in real 
time. As regards the trunk's position/orientation, while 
the first to third embodiments time series data creat- 
ed offline in advance so as to satisfy the target ZMP 
trajectory is stored in the macro-attitude controller, in 

55 the fourth embodiment the trunk's position/orienta- 
tion are generated in real time on the basis of the feet 
positions/orientations of the geometric and dynamic 
model, the target ZMP trajectory and the aforesaid 
moment Mmdl. 

11 
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In the robot model of the fourth embodiment, the 
feet positions/orientations are transformed to the pos- 
itions/attitudes obtained by coordinate-rotating the 
foot positions/orientations supplied by the foot trajec- 
tory generator by the amount of the virtual ground's 
inclinatory angular command Gctrl. In the robot mod- 
el, the trunk's position and orientation are obtained by 
kinematic calculation such that the ground reaction 
force moment Mact at the target ZMP position be- 
comes the moment Mmdl, and the displacements of 
the respective joints are simultaneously calculated. 
When Mmdl is zero, the ZMP of the robot model co- 
incides with the target ZMP position. When Mmdl is 
not zero, however, a moment is produced about the 
target ZMP position, meaning that the target ZMP 
position is no longer at the robot model's ZMP position 
and thus that the robot model's ZMP position is shift- 
ed away from the target ZMP position. 

It is assumed that the model's ground surface is 
able to produce all types of overall ground reaction 
forces. Namely, the model's ground surface is as- 
sumed to be capable of producing an attractive force 
such that, for example, the model's ground contact 
surface will not separate from the ground (the prescri- 
bed constraint condition between the leg and the 
ground will be maintained) even if during level ground 
walking the ZMP moves outside the minimum convex 
polygon including the ground contact surface. As a re- 
sult of this assumption, it becomes possible to pro- 
duce a large restoration of a magnitude not attainable 
by manipulation of only the actual robot's overall 
ground reaction force. By increasing the feedback 
gain it therefore becomes possible to converge the 
error on zero in a short period of time. That is to say, 
in the actual robot not only the joint displacements but 
also the inclination follow those of the model with high 
response. It should be noted, however, that the com- 
ponent of the model's overall ground reaction force 
normal to the ground should be directed upward (in 
the positive direction) because otherwise the actual 
robot is liable to rise off the ground momentarily. 

The macro-attitude controller of the fourth em- 
bodiment determines the next footfall's position and 
timing, the target ZMP trajectory and other walking 
pattern's parameters on the basis of the behavior of 
the model, the inclinatory error of the actual robot rel- 
ative to the model, and the like. Since the first to third 
embodiments are provided with the target walking 
pattern, they need only to determine the error amount 
from the target walking pattern. Since the fourth em- 
bodiment does not have the target walking pattern, 
however, the determination of the walking pattern's 
parameters becomes difficult. For making the deter- 
mination as simple as possible, it suffices to focus on 
macro-state quantities such as the angular momen- 
tum and the position of the center of gravity of the ro- 
bot as a whole as viewed from a given reference point. 
If the local attitude control does not much affect these 



macro-state quantities, it is possible to predict the fu- 
ture behavior from only the macro-state quantities 
and to determine the walking pattern's parameters so 

5 that the macro-state quantities do not diverge. To be 
specific, it is possible to use the earlier mentioned 
method employed for footfall's positional control in bi- 
ped stilt robots or to use a method in which simula- 
tions are conducted for advance learning of footfall's 

10 positions appropriate for the macro-state quantities. 

In the robot according to the fourth embodiment, 
the reaction of the free leg can be ignored during slow 
walking and the robot can be simulated as a simple 
inverted pendulum. It is known that when the robot 

15 can be simulated as a simple inverted pendulum the 
locus in the case where the angular momentum is 
represented on the vertical axis and the position of 
the center of gravity on the horizontal axis is a hyper- 
bola. The robot's behavior can therefore be analyti- 

20 cally predicted and the footfall's position determina- 
tion can also be conducted analytically. This is the 
method used in the aforesaid footfall's position con- 
trol of the biped stilt robot. When the robot according 
to the fourth embodiment walks rapidly (at 3 km/h, for 

25 example), the free leg's reaction cannot be ignored 
and the simulation as a simple inverted pendulum be- 
comes impossible. In this case, rather than using an 
analytical determination method it is more effective to 
use one in which footfall's positions appropriate for 

30 the macro-state quantities are learned in advance. 
The learning can be achieved by artificial intelli- 
gence, fuzzy logic, neuro networks or the like. 

Steps S900 to S910 of Figure 23 make up the 
main routine, steps S1000 to S1014 of Figure 24 a 

35 subroutine of the attitude control procedures of the 
main routine, and steps S1100 to S1106 of Figure 25 
a subroutine of the compliance and attitude stabiliza- 
tion control procedures of the main routine of the 
fourth embodiment. The subroutine flow chart of Fig- 

40 ure 25 is the same as the corresponding one of the 
first embodiment. 

Since the fourth embodiment has a more precise 
dynamic model than the first embodiment, it provides 
more accurate attitude control. It is particularly char- 

45 acterized in that it is able to resist tip-over and achieve 
recovery even when the robot is exposed to a large 
disturbance which greatly destabilizes its attitude. 

Figure 26 is a block diagram showing the control 
according to af ifth embodiment of the invention. This 

so embodiment does not have the macro- attitude con- 
troller and a walking pattern's parameter determining 
section merely outputs a series of walking pattern's 
parameters preset to enable long-term stable walking 
under disturbance free conditions. 

55 The fifth embodiment is provided with a first and 

a second geometric/dynamic models. The initial 
states of the f irst and second models are set to be the 
same as the initial states of the aforesaid series of the 
preset walking patterns. The first model is supplied 

12 
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with the target foot trajectories and the target ZMP 
trajectory and generates the trunk trajectory so as to 
satisfy the target ZMP trajectory. Since, as men- 
tioned above, the walking pattern's parameter deter- 
mining section outputs a series of walking pattern 
parameters preset to enable long-term stable walking 
under disturbance free conditions, the behavior of the 
first model is a faithful reproduction of the walking 
pattern. In addition to being supplied with the target 
foot trajectories and the ZMP trajectories, the second 
model is also supplied with a manipulated variable for 
converging the trunk's positional error between the 
first and second models on zero and with a manipu- 
lated variable for converging the attitude inclinatory 
error between the second model and the actual robot 
on zero. The manipulated variables are applied as the 
aforesaid moment Mmdl about the target ZMP posi- 
tion. 

In the embodiment, the demand value Merrdmd 
obtained by passing the inclinatory angular error Gerr 
of the actual robot relative to the second model 
through the actual robot's inclinatory error control's 
feedback rule H1 (S) of the PD type or the like and 
the demand value Mmdldmd obtained by passing the 
positional error between the first and second models 
through a model's trunk position/velocity control's 
feedback rule H2 (S) concerning the trunk's position- 
al error between the first and second models are 
passed through a limiter and distributor of the same 
type as used in the second embodiment and distrib- 
uted to the moment Mactcomm and the moment 
Mmdl. 

The ground reaction force moment' control's 
feedback rule calculates the ground reaction force 
moment's feedback manipulated variable 9comp from 
the actual ground reaction force moment Mact about 
the target ZMP position. The virtual ground's inclina- 
tory angular command Gctrl is obtained by adding the 
manipulated variable 9comp to the product of moment 
Mactcomm and the compliance constant Kcomp. As 
for the feet positions of the second model, the feet 
positions generated by the target foot trajectory gen- 
erator are coordinate-rotated about the target ZMP 
position by the amount of the virtual ground's inclin- 
ation angular command Gctrl. 

The first and second models are both the same 
as the model of the fourth embodiment. However, 
since the first model is not provided with the moment 
Mmdl and the virtual ground's inclinatory angular 
command ectrl, when the first and second models are 
to be created using the same program, therefore, it 
suffices to input the values of the moment Mmdl and 
command Gctrl to the first model zero. 

Steps S1200 to S1210 of Figure 27 make up the 
main routine, steps S1300 to S1318 of Figure 28 a 
subroutine of the attitude control procedures of the 
main routine, and steps S1400 to S1406 of Figure 29 
a subroutine of the compliance control procedures of 



the main routine of the fifth embodiment. These flow 
charts are substantially the same as those of the sec- 
ond embodiment. 

5 Although, in the second embodiment, the trunk 

trajectory was generated in advance, in the fifth em- 
bodiment, on the other hand, the trunk trajectory is 
generated in real time in the first model so as satisfy 
the target ZMP trajectory. The effect of the fifth em- 

10 bodiment is essentially the same as that of the sec- 
ond embodiment However, since its dynamic model 
is more faithful to the robot's behavior, it achieves 
more precise attitude stabilization control and is able 
to restore the model's attitude even when it is has 

15 been greatly destabilized. 

Figure 30 is a diagram showing the control ac- 
cording to a sixth embodiment of the invention. The 
sixth embodiment combines the controls of the fourth 
and fifth embodiments for achieving model's attitude 

20 stabilization. In the sixth embodiment, the restriction 
of the angular momentum, the position of the center 
of gravity and other macro-state quantities of the sec- 
ond model within certain ranges is achieved against 
instantaneous and minute disturbances mainly by 

25 controlling the moments applied to the actual robot 
and the second model, and is achieved against large 
disturbances of long duration mainly by controlling 
the change in footfall's position and time. 

At the transition between walking patterns, the 

30 first model is matched to the state of the second mod- 
el. (In other words, at the beginning of the walking 
pattern of each walking step the initial state of the first 
model is forcibly adjusted to the state of the second 
model at that time.) Unless this is done, a problem 

35 arises when the inclinatory error of the actual robot 
relative to the second model is made large by a large 
disturbance of long duration acting on the actual ro- 
bot. Specifically, when a large amount of the moment 
Mactcomm is applied to the second model for correct- 

40 ing the inclinatory error, the control for converging 
the attitude error between the first and second mod- 
els does not produce adequate results, so that the at- 
titude error between the first and second models di- 
verges. If the initial state of the first model is rapidly 

45 matched to the second model, however, the manipu- 
lated variable for converging the attitude error be- 
tween the first and second models is liable to change 
abruptly. The feedback gain of the control for con- 
verging the attitude error between the first and sec- 

50 ond models should therefore preferably be continu- 
ously reduced just before the transition from one 
walking pattern to the next. 

In the fourth embodiment, the accuracy with 
which the future behavior of the model can be predict- 

55 ed is degraded by the fact that the behavior of the 
model becomes unpredictable when the actual robot 
is subjected to an unpredictable disturbance. In the 
sixth embodiment, the second model is converged on 
the behavior of the first model within the period of 

13 
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each walking step. Since the behavior of the first 
model is not affected by a disturbance acting on the 
actual robot within a single walking step, the behavior 
of the first model can be predicted with certainty. 5 
Thus, even if the behavior of the second model 
changes because of a disturbance acting on the ac- 
tual robot it will soon be converged on the behavior 
of the first model and, as a result, the unpredictability 
of the second model's behavior is reduced. In other 10 
words, in the sixth embodiment, the manipulation of 
the footfall position and other walking pattern's para- 
meters produce a more reliable attitude stabilization 
effect than it does in the fourth embodiment. 

Steps S1500 to S1510 of Figure 31 make up the 15 
main routine, steps S1600 to S1620 of Figure 32 a 
subroutine of the attitude control procedures of the 
main routine, and steps S1700 to S1706 of Figure 33 
a subroutine of the compliance control procedures of 
the main routine of the sixth embodiment. 20 

Since the unpredictability of the model's future 
behavior is reduced in the sixth embodiment than in 
the fourth, more reliable long-term attitude stabiliza- 
tion can be realized. 

In the foregoing, it was explained in the sixth em- 25 
bodiment that the error between the first and second 
models is forcibly made zero at the transition between 
walking patterns. It is also possible to apply the mo- 
ment to the first model for converging the error be- 
tween the first and second models on zero at other 30 
times when the error becomes large. When the mo- 
ment is applied to the first model, however, the result- 
ing error of the trunk's position tends to destabilize 
the attitude of the first model. This attitude destabili- 
zation can be prevented by correcting thefootfall pos- 35 
ition and other walking pattern parameters at the 
same time as applying the moment to the first model. 
If the footfall position or time is greatly changed im- 
mediately before footfall, however, the leg's behavior 
changes abruptly. Thus the acceptable amount of 40 
change in the footfall position and time grows smaller 
as the amount of time before footfall decreases. 
Therefore, when the moment is applied to the first 
model and the footfall position or the like is corrected 
accordingly, it is preferable to reduce the maximum 45 
permissible amount of the moment to be applied as 
the time to footfall grows shorter. 

Further, the dynamic models of the fifth and 
sixth embodiments are configured in two stages. It is 
also possible to increase the number of model stages so 
to three or more and control the error between the ac- 
tual robot and the models and the error among the 
models. The higher the level of the model (i.e. the 
more remote the model is from the actual robot 
among the tandem models), the less it is affected by 55 
disturbances acting on the actual robot and, accord- 
ingly, the higher is its behavior prediction's accuracy. 
Therefore, if the footfall's positional correction 
amount is calculated on the basis of the behavior of 



the higher level model, it will be possible to secure the 
attitude stability of the higher level model and, more- 
over, since the control of the moment of a lower level 
model causes it to follow the model above it, it be- 
comes possible as a result to ensure the attitude sta- 
bility of the actual robot and all the models. The same 
applies when perturbation models are used. 

Some additional explanation will now be given re- 
garding the limiter and distributor used in the second 
embodiment and the like. The purpose of the limiter 
and distributor is to distribute the manipulated vari- 
ables Mmdl and Mactcomm while maintaining the 
ground contactability of the actual robot. The limiter 
is not restricted to that mentioned in connection with 
the second embodiment. The following modified ar- 
rangements can also be used, alone or in combina- 
tions. 

a. Placing a prescribed limit on the manipulated 
variables calculated by the control rule in terms 
of moment. 

b. Varying the limit value of the manipulated va- 
riables calculated in terms of moment in accor- 
dance with the actual or designed value of the 
component of the ground reaction force vertical 
(normal) to the ground. 

In this case, since 
Error amount of ZMP = Moment/ Vertical 
component of ground reaction force the best 
ground contactability is obtained by varying the 
limiter as a function of the product obtained by 
multiplying the distance from the target ZMP pos- 
ition to the boundary of the ZMP permissible re- 
gion by the vertical component of the ground re- 
action force. 

c. Varying the limit value of the manipulated va- 
riables calculated in terms of moment based on 
the distance from the target ZMP position to the 
boundary of the ZMP permissible region. 

Since the ZMP permissible region is large 
during two-leg support periods during level 
ground walking, this arrangement enables the 
production of a large restoration force in the ac- 
tual robot 

d. Once converting the manipulated variables 
calculated by the control rule to positional error 
of the ZMP and limiting the result according to the 
distance from the target ZMP position to the 
boundary of the ZMP permissible region. 

e. In the case where the manipulated variables 
are calculated by the PD control rule, applying a 
limit to the manipulated variables calculated from 
the P component and not applying a limit to a D 
component exhibiting an oscillation damping ef- 
fect, or vice versa. 

f. As regards the manipulated variable of the mo- 
ment Mactcomm, passing the moment Mact- 
comm through a limiter whose limit value can be 
varied in accordance with the actual ground reac- 
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tion force moment Mact, so as to prevent Mact 
from exceeding its limit value. 

g. As regards the same variable, multiplying any 
excess of Mact over its limit value by a gain and 
subtracting the product from the moment Mact- 
comm. 

h. Also as regards the same variable, calculating 
the ZMP position of the actual robot from the de- 
tected actual robot's ground reaction force and 
not increasing the absolute value of the moment 
Mactcomm further if the calculated ZMP position 
of the actual robot is likely to move outside the 
ZMP permissible region. 

it is also possible to use simple proportional dis- 
tribution for the distribution, as shown by Eq. 24. 

Mactcomm = a • Merrdmd + Mmdldmd 
Mmdl = (a - 1) - Merrdmd + Mmdldmd Eq. 24 
where a is constant. 

The distribution can also be conducted as a func- 
tion of the frequency band. For example, if the mo- 
ment Mactcomm is applied only with the high- 
frequency components of the demand values 
Merrdmd and Mmdldmd, and the low-frequency com- 
ponent thereof is applied to the moment Mmdl, the 
variable for manipulating the ground reaction force of 
the actual robot will be held to a low value even with- 
out passing it through a limiter and, as a result, the 
ground contactability will be enhanced. 

Furthermore, in the second embodiment, the 
manipulated variables are obtained by calculating the 
demand value(s) by the feedback rule and then pass- 
ing through the limiter and distributor. However, the 
order of these procedures can be reversed or the lim- 
iter can be inserted on either side of the distributor. 

Some additional explanation will now be given re- 
garding the walking pattern referred to in the descrip- 
tion of the first to sixth embodiments. The walking pat- 
tern is ordinarily described in terms of constraint con- 
ditions including the conditions relating to the ground 
reaction force required for securing ground contact, 
and the joint displacements are determined to satisfy 
the described conditions. As the constraint conditions 
there are used the foot trajectories, the trunk's orien- 
tation, an equation for determining the height of the 
trunk that does not cause unreasonable joint dis- 
placement's behavior, and the like. Atypical condition 
regarding the ground reaction force is, for example, 
the ZMP permissible region (the region within which 
it is permissible for the ZMP to exist) during level 
ground walking. The ZMP permissible region is the 
minimum convex polygon including the ground con- 
tact surface. The target ZMP trajectory is established 
within this region and the walking pattern is generat- 
ed so that the ZMP kinematically calculated from the 
walking pattern by use of an Euler-Newton equation 
or the like coincides with the target ZMP trajectory. 
Instead of conditions relating to the ground reaction 
force, it is possible to use conditions relating to the 



force or moment acting on a specified point such as 
the ankle. This is possible because during a one-leg 
support period in a biped walking robot, for example, 
5 the fact that the ankle of the supporting leg is near 
the ground means that the ankle torque is closely re- 
lated to the ground reaction force moment acting on 
the point of vertical projection of the ankle on the 
ground. 

10 Moreover, in the dynamic model it is necessary to 

calculate the model behavior which satisfies the tar- 
get ground reaction force conditions. By "target 
ground reaction force conditions" is meant, for exam- 
ple, the generation of the moment Mmdl. Since the 

15 model's foot positions/orientations and the trunk's ori- 
entation are supplied from the walking pattern gener- 
ator, however, in the illustrated embodiment relating 
to the robot whose legs each have six degrees of free- 
dom, the displacements of all joints are determined 

20 once the trunk's orientation has been decided. Of the 
trunk's positional parameters, the height is, for exam- 
ple, decided such that the instantaneous displace- 
ment or its velocity or its acceleration etc. of the knees 
or other joints does not exceed the capability of the 

25 actuators and that the reaction force normal to the 
ground does not become negative. The target ground 
reaction force conditions are therefore satisfied by 
the remaining trunk's positional parameters, namely 
the fore/aft and left/right behaviors. However, owing 

30 to the difficulty of direct, analytical calculation of 
trunk's fore/aft and left/right positions that satisfy the 
target ground reaction force conditions at each in- 
stance, the actual approach used is to impart posi- 
tions to the trunk, calculate the resulting ground re- 

35 action force, and then use the newton method or 
other such converging method of approximation to 
search for the trunk's position that satisfies the target 
ground reaction force. If high-speed computation is 
important, the number of search cycles can be limit- 

40 ed, although this introduces some amount of error. In 
the extreme, the number of search cycles can be re- 
duced to one. 

During two-leg support periods, ground contact- 
ability is not degraded even if the moment Mact is 

45 large. During such periods it is therefore possible to 
increase the limiter's limit value or to raise the gain of 
the model trunk's positional control rule and the like. 

On the other hand, if the walking environment or 
purpose makes attitude control particularly impor- 

so tant, it may become necessary to stress the accuracy 
of the footfall position. This is the case during walking 
on stairs or stepping stones, for example. In such sit- 
uations, making the control gains variable makes it 
possible to realize the degree of control precision 

55 made necessary by the particular circumstances. If 
the emphasis is on attitude stability, for example, the 
gain of the model trunk's positional (or inclinatory) 
control is lowered to reduce the demand value 
Mmdldmd and the control for converging the model's 

15 
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inclination (or trie model trunk's position) on zero is 
achieved mainly by correcting the footfall position 
and other walking parameters. This method is useful 
because by reducing moment Mactcomm so that the 
actual robot's ZMP position moves near the center of 
the ZMP permissible region, it increases the margin 
of stability. It should be noted, however, that the foot- 
fall position's accuracy is degraded because footfall 
position and other walking pattern parameters are 
drastically modified. Depending on the walking envir- 
onment and purpose, it is also possible to achieve 
precision control appropriate for the circumstances by 
adjusting the frequency characteristics of the control 
rules' gain. 

Although the first to sixth embodiments descri- 
bed in the foregoing use the compliance control as 
the actual robot's ground reaction force moment's 
feedback control, it is also possible to use torque con- 
trol instead. In cases where the ground configuration 
is known and little disturbance arises owing to ground 
irregularities (bumps and recesses) or cases where 
compliance is achieved mechanically, there is no par- 
ticular need to conduct the ground reaction force mo- 
ment's feedback control. Even if it is not conducted, 
there is no loss of the advantage of the invention. 

While the invention was described with reference 
to a biped walking robot as an example of the legged 
mobile robot, the invention can be applied not only to 
a legged robot other than the biped one, but also any 
other robots of wheeled or crawler type. 



Claims 

1. A system for controlling attitude stabilization of a 
mobile robot, including: 

at least one mathematical model describ- 
ing a behavior of the robot; and 

control means for controlling a behavior of 
the robot such that it follows the model's behav- 
ior; 

wherein the system further comprises: 

first means for determining error between 
the model's behavior and the robot's actual be- 
havior; and 

second means for determining a manipu- 
lated variable in response to the error and supply- 
ing it to the model such that the model's behavior 
is modified in response to the manipulated vari- 
able; and wherein 

said control means controls the behavior 
of the robot such that it follows the model's modi- 
fied behavior. 

2. A system according to claim 1 , further including: 

at least a second mathematical model de- 
scribing a behavior of the robot; wherein 

said first means determines an error be- 



tween the behavior of one of the models and the 
robot's actual behavior; and 

said second means determines the ma- 
5 nipulated variable in response to the error and 

supplies it to one of the models such that the 
model's behavior is modified in response to the 
manipulated variable. 

w 3. A system according to claim 1 or 2, further includ- 
ing; 

at least a second mathematical model de- 
scribing a behavior of the robot; wherein 

said first means determines an error be- 
ts tween the first model's behavior and the second 
model's behavior; and 

said second means determines the ma- 
nipulated variable in response to the error and 
supplies it to one of the models such that the 
20 model's behavior is modified in response to the 

manipulated variable. 

4. A system according to any of preceding claims 1 
to 3, wherein said second means determines a 

25 second manipulated variable in response to the 

error and supplies it to the robot and/or model. 

5. A system according to any of preceding claims 1 
to 4, wherein the robot is a legged-mobile robot 

30 walking on the ground, and said model describes 

the behavior of the robot defined in terms of a de- 
sired walking pattern established at least in such 
a manner that ground reaction force acting on the 
robot is to be a desired value, and said second 

35 means; determines the manipulated variable in 

terms of the ground reaction force in response to 
the error and supplies it to the model such that 
the model's behavior is modified toward a state 
in which the error is decreased. 

40 

6. A system for controlling attitude stabilization of a 
legged mobile robot walking on the ground, com- 
prising: 

at least one mathematical model simulat- 
45 ing a behavioral variation from a desired walking 

pattern established at least in such a manner that 
ground reaction force acting on the robot is to be 
a value such that the robot's leg does not lose 
contactability during walking, and a variation from 
so the designed ground reaction force; 

first means for determining an attitude er- 
ror between the desired walking pattern and the 
robot; 

second means for determining a manipu- 
55 lated variable in terms of the ground reaction 

force and supplying it to the model such that the 
attitude error decreases; 

third means for determining a behavioral 
variation of the model to modify the desired walk- 

16 
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ing pattern in response to the determined beha- 
vioral variation; and 

control means for controlling joints of the 
robot in accordance with the modified desired 
walking pattern. 

7. A system for controlling attitude stabilization of a 
legged mobile robot walking on the ground, com- 
prising: 

at least one dynamic model simulating a 
behavioral variation from a desired walking pat- 
tern established at least in such a manner that 
ground reaction force acting on the robot is to be 
a value such that the robot's leg does not lose 
contactability during walking, and a variation from 
the designed ground reaction force; 

at least one geometric or kinematic model 
simulating the mechanical structure of the robot; 

first means for determining an attitude er- 
ror between the geometric or kinematic model 
and the robot; 

second means for determining a manipu- 
lated variable in terms of the ground reaction 
force and supplying it to the dynamic model such 
that the attitude error decreases; 

third means for determining a behavioral 
variation of the dynamic model to modify the de- 
sired walking pattern on the geometric or kine- 
matic model in response to the determined beha- 
vioral variation; and 

control means for controlling joints of the 
robot in accordance with a behavior of the geo- 
metric or kinematic model. 

8. A robot according to claim 6 or 7, wherein the ro- 
bot is a legged mobile robot having a trunk and 
at least one leg connected to the robot, and said 
first means determines an inclinatory error of the 
trunk as the attitude error. 

9. A system according to claim 6, 7 or 8, wherein 
said third means additionally modifies the de- 
sired walking pattern such that a position at which 
the robot's leg is to be landed is varied in re- 
sponse to the detected behavioral variation such 
that the robot's attitude stabilization is enhanced. 

10. A system according to any of claims 6 to 9, 

wherein said second means determines a 
second manipulated variable in terms of the 
ground reaction force and supplies it to the robot 
and/or the model such that the robot's attitude 
stabilization is enhanced. 

11. A system for controlling attitude stabilization of a 
legged mobile robot having a trunk and a plurality 
of legs each connected to the trunk and walking 
on the ground; comprising; 



generating means for generating a desired 
walking pattern of the robot at least in such a 
manner that a ground reaction force moment act- 
5 ing on the robot is to be a desired value; 

a first mathematical model made up of an 
inverted pendulum and simulating dynamics of 
the robot; 

a second mathematical model simulating 
10 the mechanical structure of the robot such that 

the second model is assumed to walk on the 
ground in accordance with the desired walking 
pattern; 

first means for determining an inclinatory 
15 error of the robot relative to the second model; 

second means for determining a manipu- 
lated variable in terms of moment in response to 
the determining inclinatory error and supplying it 
to the first model such that the inverted pendu- 
20 lum is rotated about its pivot point by an amount 

corresponding to the manipulated variable; 

third means for determining positional va- 
riation of an upper portion of the inverted pendu- 
lum resulting from the rotation and modifying the 
25 motion of the second model in response to the 

determined positional variation; and 

control means for controlling joints of the 
robot such that it follows the motion of the second 
model. 

30 

12. A system according to claim 11, further including: 
fourth means for determining a positional 
error between the desired position and actual 
position of the ground reaction force moment; 
35 wherein 

said third means additionally modifies the 
motion of the second model in response to the 
determined positional error. 

40 13. A system according to claim 11 or 12, wherein 
said third means additionally modifies the motion 
of the second model such that a position at which 
the robot's leg is to be landed is varied in re- 
sponse to the determined positional variation 

45 such that the robot's attitude stabilization is en- 

hanced. 

14. A system according to any of preceding claims 11 
to 13, wherein said second means determines a 
so second manipulated variable in terms of the 

ground reaction force moment and supplies it to 
the second model such that the robot's attitude 
stabilization is enhanced. 

55 15. A system according to any of preceding claims 11 
to 14, wherein said second means determines 
the manipulated variable less than a predeter- 
mined limit value. 
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16. Asystem according to any of preceding claims 11 
to 15, wherein the first and second models are 
prepared independently. 

5 

17. Asystem according to any of preceding claims 11 
to 15, wherein the first and second models are 
combined to be a composite model. 

18. Asystem according to any of preceding claims 11 10 
to 15, wherein the first and second models are 
combined to be composite models. 

19. Asystem according to claim 18, wherein the said 

first means determines an inclinatory error of the is 
trunk between the models. 

20. Asystem according to any of preceding claims 11 
to 19, wherein the desired walking pattern is de- 
fined in terms of positions and orientations of the 20 
trunk and distal end of the legs. 

21. Asystem according to any of preceding claims 1 
to 20, wherein the robot is a biped robot. 

25 
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Actual robot 



When the actual ZMP position shifts behind 
the target ZMP position (i.e. when the target 
ZMP position shifts ahead of the actual ZMP 
position), a moment acting to cause the robot 
to fall forward is produced 



M = x*F 



Target ZMP position 
\ (determined by model) 



FIG. 3 

Actual robot 



Ground 
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(determined by model) 
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Target ZMP position 
(determined by model) 
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FIG. 10 



CstarO 
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Initialize dynamic model to upright position 



S14 



Repeat until walking 
step counter reaches 
target number of 
walking step 
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Set target walking pattern 
for current cycle 



SJ6 



Define difference between corrected 
and initial footfall positions of target 
walking pattern of preceding cycle as 
Axfoottotal 



Subtract Axfoottotal from free 
leg's initial target position of 
walking pattern in current cycle 
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Subtract Axfoottotal/htrunk from 
dynamic model's inclinatory angle Omdl 



Repeat for 
number of 
sampling 
times per one 
walking step 
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Wait for one 
cycle to pass 



Conduct attitude 
control subroutine 
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FIG. 11 



CentrO 



Measure actual robot's inclination to detect 
flerr and aerr relative to geometric model 



S100 



Calculate Mmdl from flerr and werr using actual 
robot's inclinatory error control's feedback rule 
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Apply Mmdl to pivot point of dynamic model 
and calculate flmdl and wmdl 
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Calculate trunk's positional error ax from: 

ax = 0rndl X htrunk (where htrunk is trunk's height) 
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If there is adequate time before footfall, predict behavior 
of dynamic model, calculate Axfoot, and add to initial 
target footfall position to correct it 



Calculate target ZMP position from target ZMP 
trajectory's parameters 
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Calculate positions/orientations of both feet from target 
footfall position and other foot trajectory's parameters 



Add 6mdl X htrunk to initial trunk position of target 
walking pattern to determine as target trunk position 
(trunk's orientation is held as it is) 
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Conduct compliance control subroutine 
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Calculate joint displacements of geometric model from target 
trunk position/orientation and corrected feet's positions/orientations 



Control actual robot's joint displacements 
to follow geometric model's displacements 
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FIG. 12 

CLnTRO S200 



Read detection values of six- dimensional 
force and torque sensors 



S202 



Calculate actual ground reaction force moment Mact 
about target ZMP position from detected values 



Calculate coordinate rotation angle 0ctrl as: 
Coordinate rotation angle flctrl = 
Actual ground reaction force moment Mact 
x Compliance control gain 
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Rotate target positions/orientations of both feet 
about target ZMP position by sctrl, putting them in 
model's feet positions/orientations 
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FIG. 15 
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FIG. 16 



Measure actual robot's inclination to detect 
flerr ond aerr relative to geometric model 
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Calculate Merrdmd from flerr and werr using actual 
robot's inclinatory error control's feedback rule 
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model's inclinatory control's feedback rule 



Determine Mmdl and Mactcomm from Merrdmd and 
Mmdldmd through limiter/distributor 
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Apply Mmdl to pivot point of dynamic model 
and calculate flmdl and wmdl 
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Calculate trunk's positional error ax from: 

Ax = flmdl X htrunk (where htrunk is trunk's height) 



Calculate target ZMP position from target ZMP 
trajectory's parameters 



S412 
S414 



1 



Calculate positions/orientations of both feet from target 
footfall position and other foot trajectory's parameters 



Add flmdl x htrunk to initial trunk position of target 
walking pattern to determine as target trunk position 
(trunk s orientation is held as it is) 
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FIG. 17 
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Read detection values of six- dimensional 
force and torque sensors 
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Calculate actual ground reaction force moment Mact 
about target ZMP position from detected values 



Calculate coordinate rotation angle ectrl as: 
Coordinate rotation angle fictrl = 
Actual ground reaction force moment Mact 
X Compliance control gain 
+ Moment Mactcomm 
x Compliance constant Kcomp 
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Rotate target positions/orientations of both feet 
about target ZMP position by ectrl, putting them in 
model's feet positions/orientations 
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FIG. 19 
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Initialize dynamic model to upright position 
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Repeat until walking 
step counter reaches 
target number of 
walking step 
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Set target walking pattern 
for current cycle 



S606 



Define difference between corrected 
and initial footfall positions in target 
walking pattern of preceding cycle as 
Axfoottotal 



Subtract Axfoottotal from free 
leg's initial target position of 
walking pattern in current cycle 
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FIG. 20 



CIntrO 



Measure actual robot's inclination to detect 
flerr and &>err relative to geometric model 



Calculate Merrdmd from 0err and werr using actual 
robot's inclinatory error control's feedback rule 



Calculate Mmdldmd from 0mdl and wmdl using 
model's inclinatory error control feedback's rule 
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Calculate Mmdl and Mactcomm from Merrdmd and 
Mmdldmd through limiter/distributor 
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Apply Mmdl to pivot point of dynamic model 
and calculate 0mdl and wmdl 
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Calculate trunk's positional error ax from: 

ax = 0mdl x htrunk (where htrunk is trunk's height) 
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If there is adequate time before footfall, predict behavior 
of dynamic model and correct initial target footfall position 



Calculate target ZMP position from target ZW)r~S714 
trajectory's parameters 
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Calculate positions/orientations of both feet from target 
footfall position and other foot trajectory's parameters 



Add 0mdl x htrunk to initial trunk position of target 
walking pattern to determine as target trunk position 
(trunk s orientation is held as it is) 
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Conduct compliance control subroutine 



5720 



5722 
[ 



Calculate joint displacements of geometric model from target 
trunk position/orientation and corrected feet's positions/orientations 



Control actual robot's joint displacements 
to follow geometric model's displacements 
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FIG. 21 
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Read detection values of six-dimensional 
force and torque sensors* 
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Calculate actual ground reaction force moment Mact 
about target ZMP position from detected values 



Calculate coordinate rotation angle Gctrl as: 
Coordinate rotation angle 0ctrl = 
Actual ground reaction force moment Mact 
x Compliance control gain 
+ Moment Mactcomm 

x Compliance constant Kcomp 
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Rotate target positions/orientations of both feet 
about target ZMP position by flctrl, putting them in 
model's feet positions/orientations 
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FIG. 23 
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Initialize geometric/dynamic model to upright position 



Repeat until walking 
step counter reaches 
target number of 
walking step 
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current states 
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number of 
sampling 
times per one 
walking step 
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Wait for one 
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FIG. 24 
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Measure actual robot's inclination to detect 
0err and werr relative to model 



Calculate Mmdl from flerr and aerr using actual 
robot's inclinatory error control's feedback rule 
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If there is adequate time before footfall, predict behavior 
of model and correct initial target footfall position 



Calculate target ZMP position from target ZMP 
trajectory's parameters 
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Calculate positions/orientations of both feet from target 
footfall position and other foot trajectory's parameters 



Conduct compliance control subroutine 
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Calculate model's trunk position such that moment Mmdl 
is generated about target ZMP position to determine 
joint displacements of model 



Control actual robot's joint displacements 
to follow model's displacements 
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FIG. 25 
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Read detection values of six-dimensional 
force and torque sensors 
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Calculate actual ground reaction force moment Mact 
about target ZMP position from detected values 



Calculate coordinate rotation angle ectrl as: 
Coordinate rotation angle 0ctrl = 
Actual ground reaction force moment Mact 
x Compliance control gain 
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Rotate target positions/orientations of both feet 
about target ZMP position by flctrl, putting them 
model's feet positions/orientations 
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FIG. 27 
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Initialize 1st and 2nd models to upright position 



Repeat until walking 
step counter reaches 
target number of 
walking step 



— r~ 

S1202 



S12p4 



Set target walking pattern 
for current cycle in 
predetermined data 



Repeat for 
number of 
sampling 
times per one 
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FIG. 28 
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Measure actual robot's inclination to detect 
0err and werr relative to 2nd model 
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Calculate Merrdmd from flerr and werr using actual 
robot's inclinatory error control's feedback rule 
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Calculate Mmdldmd from 1st model's positional and velocity 
errors relative to 2nd model using model's trunk position/ 
velocity control's feedback rule 



Calculate Mmdl and Mactcomm from Merrdmd and 
Mmdldmd through limiter/distributor 



Calculate target ZMP position from target ZMP 
trajectory's parameters 
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Calculate positions/orientations of both feet from target 
footfall position and other foot trajectory's parameters 



Conduct compliance control subroutine for 2nd model 



Calculate 1st model's trunk position 
satisfying target ZMP position to 
determine 1st model's joint displacements 
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Calculate 2nd model's trunk position such that moment Mmdl 

is generated about target ZMP position to determine 

joint angles of 2nd model 



Control actual robot's joint displacements 
to follow 2nd model's joint angles 
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FIG. 29 
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Read detection values of six-dimensional 
force and torque sensors 
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Calculate actual ground reaction force moment Mact 
about target ZMP position from detected values 



Calculate coordinate rotation angle flctrl as-. 
Coordinate rotation angle flctrl = 
Actual ground reaction force moment Mact 
X Compliance control gain 
+ Moment Mactcomm 

x Compliance constant Kcomp 
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about target ZMP position by flctrl, putting them 
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FIG. 31 
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FIG. 32 
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Calculate Mmdl and Mactcomm from Merrdmd and 
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If there is adequate time before footfall, predict behavior 
of model and correct initial target footfall position 



Calculate target ZMP position from target ZMP 
trajectory's parameters 
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Calculate positions/orientations of both feet from target 
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FIG. 33 
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Read detection values of six-dimensional 
force and torque sensors 



Calculate actual ground reaction force moment Mact 
about target ZMP position from detected values 



Calculate coordinate rotation angle ectrl as: 
Coordinate rotation angle 0ctrl = 
Actual ground reaction force moment Mact 
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+ Moment Mactcomm 
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